#include "PCFuncSimd.h"

#include <limits.h>
#include "float_def.h"
#include <stdio.h>
#include <math.h>
#include "common.h"
#include <string.h>
#include <cstdint>

// simd_match8 r0, s0, s1
bool simd_match8(Reg32 *dst, Reg128 *opt1, Reg128 *opt2)
{
	dst->word = 0;
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (int i = 0; i < n; i++)
	{
		dst->word += (opt1->byte[i] < opt2->byte[i])
						 ? (opt2->byte[i] - opt1->byte[i])
						 : (opt1->byte[i] - opt2->byte[i]);
	}

	return true;
}
bool simd_match(Reg32 *dst, Reg128 *opt1, Reg128 *opt2)
{
	Reg128 tmp = {0};
	U32 cnt = 0;
	size_t n = sizeof(Reg128) / sizeof(U64);
	for (int i = 0; i < n; i++)
	{
		tmp.dword[i] = opt1->dword[i] ^ opt2->dword[i];
		while (tmp.dword[i])
		{
			if (tmp.dword[i] & 1)
				cnt++;
			tmp.dword[i] >>= 1;
		}
	}
	dst->word += cnt;

	return true;
}
// simd_unpack8 s1, d0, d1
bool simd_unpack8(Reg128 *dst, Reg64 *opt1, Reg64 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);

	for (int i = 0; i < n; i++)
	{
		dst->byte[i] = i % 2 ? opt2->byte[(i - 1) / 2] : opt1->byte[i / 2];
	}
	return true;
}

// simd_unpack16 s1, d0, d1
bool simd_unpack16(Reg128 *dst, Reg64 *opt1, Reg64 *opt2)
{
	size_t n = sizeof(Reg64) / sizeof(U16);
	for (int i = 0; i < n; i++)
	{
		dst->shword[2 * i] = opt1->shword[i];
		dst->shword[2 * i + 1] = opt2->shword[i];
	}
	return true;
}

// simd_add8_ext1 s0, r0, s0
bool simd_add8_ext1(Reg128 *dst, Reg32 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		// opt1低8位
		dst->byte[i] = (opt1->byte[0] + opt2->byte[i]) / 2;
	}
	return true;
}

// simd_sub8_ext1 s0, r0, s0
bool simd_sub8_ext1(Reg128 *dst, Reg32 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		// opt1低8位
		if (opt1->byte[0] >= opt2->byte[i])
			dst->byte[i] = opt1->byte[0] - opt2->byte[i];
		else
			dst->byte[i] = 0;
	}
	return true;
}

// simd_add8_ext2 s0, s1, r0
bool simd_add8_ext2(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		// opt2低8位
		dst->byte[i] = (opt2->byte[0] + opt1->byte[i]) / 2;
	}
	return true;
}

// simd_sub8_ext2 s0, s1, r0
bool simd_sub8_ext2(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		// opt2低8位
		if (opt1->byte[i] >= opt2->byte[0])
			dst->byte[i] = opt1->byte[i] - opt2->byte[0];
		else
			dst->byte[i] = 0;
	}
	return true;
}

// simd_add16_ext1 s0, r0, s1
bool simd_add16_ext1(Reg128 *dst, Reg32 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U16);

	S32 res = 0;
	for (size_t i = 0; i < n; i++)
	{
		// opt1高16位
		res = opt1->shword[1] + opt2->shword[i];
		// 检查是否有正溢出
		if (res > SHRT_MAX)
		{
			res = SHRT_MAX;
		}
		// 检查是否有负溢出
		if (res < SHRT_MIN)
		{
			res = SHRT_MIN;
		}

		dst->shword[i] = (S16)res;
	}
	return true;
}

// simd_sub16_ext1 s0, r0, s1
bool simd_sub16_ext1(Reg128 *dst, Reg32 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U16);

	S32 res = 0;
	for (size_t i = 0; i < n; i++)
	{
		// opt1高16位
		res = opt1->shword[1] - opt2->shword[i];
		// 检查是否有正溢出
		if (res > SHRT_MAX)
		{
			res = SHRT_MAX;
		}
		// 检查是否有负溢出
		if (res < SHRT_MIN)
		{
			res = SHRT_MIN;
		}

		dst->shword[i] = (S16)res;
	}
	return true;
}

// simd_add16_ext2 s0, s1, r0
bool simd_add16_ext2(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U16);

	S32 res = 0;
	for (size_t i = 0; i < n; i++)
	{
		res = opt1->shword[i] + opt2->shword[1];
		// 检查是否有正溢出
		if (res > SHRT_MAX)
		{
			res = SHRT_MAX;
		}
		// 检查是否有负溢出
		if (res < SHRT_MIN)
		{
			res = SHRT_MIN;
		}

		dst->shword[i] = (S16)res;
	}
	return true;
}

// simd_sub16_ext2 s0, s1, r0
bool simd_sub16_ext2(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U16);

	S32 res = 0;
	for (size_t i = 0; i < n; i++)
	{
		res = opt1->shword[i] - opt2->shword[1];
		// 检查是否有正溢出
		if (res > SHRT_MAX)
		{
			res = SHRT_MAX;
		}
		// 检查是否有负溢出
		if (res < SHRT_MIN)
		{
			res = SHRT_MIN;
		}

		dst->shword[i] = (S16)res;
	}
	return true;
}

// simd_mask8 r0, s0
bool simd_mask8(Reg32 *dst, Reg128 *opt1)
{
	size_t n = sizeof(Reg128) / sizeof(U8);

	dst->hword[0] = 0;
	for (size_t i = 0; i < n; i++)
	{
		if (0x80 == (opt1->byte[i] & 0x80))
			dst->hword[0] |= 1 << i;
	}
	return true;
}

// simd_mask16 r0, s0
bool simd_mask16(Reg32 *dst, Reg128 *opt1)
{
	size_t n = sizeof(Reg128) / sizeof(U16);

	dst->hword[0] = 0;
	for (size_t i = 0; i < n; i++)
	{
		if (0x8000 == (opt1->shword[i] & 0x8000))
			dst->hword[0] |= 1 << i;
	}
	return true;
}

// simd_match16 r0, s0, s1
bool simd_match16(Reg32 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U16);
	dst->word = 0;
	for (size_t i = 0; i < n; i++)
	{
		dst->word += (opt1->shword[i] < opt2->shword[i])
						 ? (opt2->shword[i] - opt1->shword[i])
						 : (opt1->shword[i] - opt2->shword[i]);
	}

	return true;
}

// simd_shuffle8 s2, s0, d1
bool simd_shuffle8(Reg128 *dst, Reg128 *opt1, Reg64 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		U8 index = (opt2->dword >> (i * 4)) & 0xF;
		dst->byte[i] = opt1->byte[index];
	}
	return true;
}

// simd_8to16 s0, d0
bool simd_8to16(Reg128 *dst, Reg64 *opt1)
{
	size_t n = sizeof(Reg64) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		dst->byte[2 * i] = opt1->byte[i];
		dst->byte[2 * i + 1] = 0;
	}
	return true;
}

// simd_8to32 s0, r0
bool simd_8to32(Reg128 *dst, Reg32 *opt1)
{
	size_t n = sizeof(Reg32) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		dst->byte[4 * i] = opt1->byte[i];
		dst->byte[4 * i + 1] = 0;
		dst->byte[4 * i + 2] = 0;
		dst->byte[4 * i + 3] = 0;
	}
	return true;
}

// simd_16to32 s0, d0
bool simd_16to32(Reg128 *dst, Reg64 *opt1)
{
	size_t n = sizeof(Reg64) / sizeof(U16);
	for (size_t i = 0; i < n; i++)
	{
		dst->shword[2 * i] = opt1->shword[i];
		dst->shword[2 * i + 1] = (opt1->shword[i] < 0) ? (S16)0xFFFF : (S16)0x0000;
	}
	return true;
}

// simd_sub16 s0, s1, s2
bool simd_sub16(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U16);
	S32 res = 0;
	for (size_t i = 0; i < n; i++)
	{
		res = opt1->shword[i] - opt2->shword[i];
		// 检查是否有正溢出
		if (res > SHRT_MAX)
		{
			res = SHRT_MAX;
		}
		// 检查是否有负溢出
		if (res < SHRT_MIN)
		{
			res = SHRT_MIN;
		}

		dst->shword[i] = res;
	}
	return true;
}

// simd_shift32 s0, s0, 立即数
bool simd_shift32(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U32);
	S64 tmp = {0};
	for (size_t i = 0; i < n; i++)
	{
		tmp = (S64)opt1->sword[i];
		if (0 <= opt2->sword)
		{
			tmp = tmp >> opt2->sword;
		}
		else
		{
			// 左移，需要检查是否会溢出
			tmp = tmp << -opt2->sword;
			if (tmp > INT_MAX)
				tmp = INT_MAX;
			if (tmp < INT_MIN)
				tmp = INT_MIN;
		}
		dst->sword[i] = (S32)tmp;
	}
	return true;
}

// simd_shift16 s0, s0, 立即数
bool simd_shift16(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U16);
	S32 tmp = {0};
	for (size_t i = 0; i < n; i++)
	{
		tmp = (S32)opt1->shword[i];
		if (0 <= opt2->sword)
		{
			tmp = tmp >> opt2->sword;
		}
		else
		{
			// 左移，需要检查是否会溢出
			tmp = tmp << -opt2->sword;
			if (tmp > SHRT_MAX)
				tmp = SHRT_MAX;
			if (tmp < SHRT_MIN)
				tmp = SHRT_MIN;
		}
		dst->shword[i] = (S16)tmp;
	}
	return true;
}

// simd_add8 s0, s1, s2
bool simd_add8(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		dst->byte[i] = (opt1->byte[i] + opt2->byte[i]) >> 1;
	}
	return true;
}

// simd_add8_16 s0, d0, d2
bool simd_add8_16(Reg128 *dst, Reg64 *opt1, Reg64 *opt2)
{
	size_t n = sizeof(Reg64) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		dst->hword[i] = (U16)(opt1->byte[i] + opt2->byte[i]);
	}
	return true;
}

// simd_max32 s0, s1, s2
bool simd_max32(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U32);
	for (size_t i = 0; i < n; i++)
	{
		dst->sword[i] =
			(opt1->sword[i] < opt2->sword[i]) ? opt2->sword[i] : opt1->sword[i];
	}
	return true;
}

// simd_min32 s0, s1, s2
bool simd_min32(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U32);
	for (size_t i = 0; i < n; i++)
	{
		dst->sword[i] =
			(opt1->sword[i] < opt2->sword[i]) ? opt1->sword[i] : opt2->sword[i];
	}
	return true;
}

// simd_max32_mak s0, s1, s2
bool simd_max32_mak(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U32);
	for (size_t i = 0; i < n; i++)
	{
		dst->sword[i] = (opt2->sword[i] <= opt1->sword[i]) ? 0xffffffff : 0x00000000;
	}
	return true;
}

// simd_min32_mak s0, s1, s2
bool simd_min32_mak(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U32);
	for (size_t i = 0; i < n; i++)
	{
		dst->sword[i] = (opt1->sword[i] < opt2->sword[i]) ? 0xffffffff : 0x00000000;
	}
	return true;
}

// simd_min16_mak s0, s1, s2
bool simd_min16_mak(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U16);
	for (size_t i = 0; i < n; i++)
	{
		dst->shword[i] = (opt1->shword[i] < opt2->shword[i]) ? 0xffff : 0x0000;
	}
	return true;
}

// simd_max8_mak s0, s1, s2
bool simd_max8_mak(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		dst->byte[i] = (opt2->byte[i] <= opt1->byte[i]) ? 0xff : 0x00;
	}
	return true;
}

// simd_min8_mak s0, s1, s2
bool simd_min8_mak(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		dst->byte[i] = (opt1->byte[i] < opt2->byte[i]) ? 0xff : 0x00;
	}
	return true;
}

// simd_enlarg8 s0, d0
bool simd_enlarg8(Reg128 *dst, Reg64 *opt1)
{
	size_t n = sizeof(Reg64) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		// dst->shword[i] = ((opt1->byte[i] << 8) & 0xff00) + opt1->byte[i];
		dst->byte[2 * i] = opt1->byte[i];
		dst->byte[2 * i + 1] = opt1->byte[i];
	}
	return true;
}

// simd_enlarg16 s0, d0
bool simd_enlarg16(Reg128 *dst, Reg64 *opt1)
{
	size_t n = sizeof(Reg64) / sizeof(U16);
	for (size_t i = 0; i < n; i++)
	{
		dst->shword[2 * i] = opt1->shword[i];
		dst->shword[2 * i + 1] = opt1->shword[i];
	}
	return true;
}

// simd_enlarg32 s0, d0
bool simd_enlarg32(Reg128 *dst, Reg64 *opt1)
{
	size_t n = sizeof(Reg64) / sizeof(U32);
	for (size_t i = 0; i < n; i++)
	{
		dst->sword[2 * i] = opt1->sword[i];
		dst->sword[2 * i + 1] = opt1->sword[i];
	}
	return true;
}

// simd_ext8 s0, r0, num
bool simd_ext8(Reg128 *dst, Reg32 *opt1, Reg32 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (size_t i = 0; i < n; i++)
	{
		dst->byte[i] = (0 == opt2->word) ? opt1->byte[0] : opt1->byte[2];
	}
	return true;
}

// simd_ext16 s0, r0, num
bool simd_ext16(Reg128 *dst, Reg32 *opt1, Reg32 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(U16);
	for (size_t i = 0; i < n; i++)
	{
		dst->shword[i] = (0 == opt2->word) ? opt1->shword[0] : opt1->shword[1];
	}
	return true;
}

// simd_ext32 s0, r0
bool simd_ext32(Reg128 *dst, Reg32 *opt1)
{
	size_t n = sizeof(Reg128) / sizeof(U32);
	for (size_t i = 0; i < n; i++)
	{
		dst->sword[i] = opt1->sword;
	}
	return true;
}

bool simd_set(Reg128 *dst, Reg32 *opt1)
{
	size_t n = sizeof(Reg128) / sizeof(U8);
	for (int i = 0; i < n; i++)
	{
		if ((opt1->hword[0] >> i) & 0x0001)
			dst->byte[i] = (U8)~0x00;
		else
			dst->byte[i] = (U8)0x00;
	}

	return true;
}

bool simd_mul(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (int i = 0; i < 8; i++)
	{
		S32 a = opt1->shword[i];
		S32 b = opt2->shword[i];
		S32 c = a * b;
		dst->shword[i] = floor(c / pow(2, 16));
	}

	return true;
}
bool simd_mac16_8(Reg32 *dst, Reg128 *opt1, Reg128 *opt2)
{
	Reg64 tmp = {0};
	tmp.sdword = dst->sword;
	for (int i = 0; i < 8; i++)
	{
		tmp.sdword += opt1->shword[i] * opt2->shword[i];
	}
	if (tmp.sdword > INT32_MAX)
		dst->sword = INT32_MAX;
	else if (tmp.sdword < INT32_MIN)
		dst->sword = INT32_MIN;
	else
		dst->sword = tmp.sdword;

	return true;
}
bool simd_fadd(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(F32);

	for (int i = 0; i < n; i++)
	{
		if (is_nan(opt1->word[i]) || is_nan(opt2->word[i]) ||
			(f_infinity_n == opt1->word[i] && f_infinity == opt2->word[i]) ||
			(f_infinity == opt1->word[i] && f_infinity_n == opt2->word[i]))
		{
			dst->word[i] = f_qnan;
		}
		else
		{
			dst->f[i] = opt1->f[i] + opt2->f[i];
		}
	}

	return true;
}

bool simd_fsub(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(F32);
	for (int i = 0; i < 4; i++)
	{
		if (is_nan(opt1->word[i]) || is_nan(opt2->word[i]) ||
			(f_infinity_n == opt1->word[i] && f_infinity_n == opt2->word[i]) ||
			(f_infinity == opt1->word[i] && f_infinity == opt2->word[i]))
		{
			dst->word[i] = f_qnan;
		}
		else
		{
			dst->f[i] = opt1->f[i] - opt2->f[i];
		}
	}

	return true;
}

bool simd_fsum(Reg32 *dst, Reg128 *opt1)
{
	double b = dst->f;
	size_t n = sizeof(Reg128) / sizeof(F32);
	for (int i = 0; i < n; i++)
	{
		if (is_nan(opt1->word[i]) || is_nan(dst->word) ||
			(f_infinity_n == opt1->word[i] && f_infinity == dst->word) ||
			(f_infinity == opt1->word[i] && f_infinity_n == dst->word))
		{
			dst->word = f_qnan;
			return true;
		}
		else
		{
			double a = opt1->f[i];
			b += opt1->f[i];
		}
	}
	dst->f = b;
	return true;
}

bool simd_fmax1(Reg32 *dst, Reg128 *opt1)
{
	size_t n = sizeof(Reg128) / sizeof(F32);

	dst->word = opt1->word[0];
	for (int i = 0; i < n; i++)
	{
		if (is_nan(opt1->word[i]) || is_nan(dst->word))
			dst->word = f_qnan;
		else if ((0x01 == ((opt1->word[i] >> 31) & 0x01)) && (0x00 == ((dst->word >> 31) & 0x01)))
			dst->word = dst->word;
		else if ((0x00 == ((opt1->word[i] >> 31) & 0x01)) && (0x01 == ((dst->word >> 31) & 0x01)))
			dst->word = opt1->word[i];
		else
		{
			dst->f = (opt1->f[i] < dst->f) ? dst->f : opt1->f[i]; // f_0 和 f_0_n比较不了
		}
	}
	return true;
}

bool simd_fmin1(Reg32 *dst, Reg128 *opt1)
{
	size_t n = sizeof(Reg128) / sizeof(F32);

	dst->word = opt1->word[0];
	for (int i = 0; i < n; i++)
	{
		if (is_nan(opt1->word[i]) || is_nan(dst->word))
			dst->word = f_qnan;
		else if ((0x01 == ((opt1->word[i] >> 31) & 0x01)) && (0x00 == ((dst->word >> 31) & 0x01)))
			dst->word = opt1->word[i];
		else if ((0x00 == ((opt1->word[i] >> 31) & 0x01)) && (0x01 == ((dst->word >> 31) & 0x01)))
			dst->word = dst->word;
		else
			dst->f = (opt1->f[i] < dst->f) ? opt1->f[i] : dst->f; // f_0 和 f_0_n比较不了
	}
	return true;
}

bool simd_fmax4(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(F32);

	for (int i = 0; i < n; i++)
	{
		if (is_nan(opt1->word[i]) || is_nan(opt2->word[i]))
			dst->word[i] = f_qnan;
		else if ((0x01 == ((opt1->word[i] >> 31) & 0x01)) && (0x00 == ((opt2->word[i] >> 31) & 0x01)))
			dst->word[i] = opt2->word[i];
		else if ((0x00 == ((opt1->word[i] >> 31) & 0x01)) && (0x01 == ((opt2->word[i] >> 31) & 0x01)))
			dst->word[i] = opt1->word[i];
		else
			dst->f[i] = (opt1->f[i] < opt2->f[i]) ? opt2->f[i] : opt1->f[i]; // f_0 和 f_0_n比较不了
	}
	return true;
}

bool simd_fmin4(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	size_t n = sizeof(Reg128) / sizeof(F32);
	for (int i = 0; i < n; i++)
	{
		if (is_nan(opt1->word[i]) || is_nan(opt2->word[i]))
			dst->word[i] = f_qnan;
		else if ((0x01 == ((opt1->word[i] >> 31) & 0x01)) && (0x00 == ((opt2->word[i] >> 31) & 0x01)))
			dst->word[i] = opt1->word[i];
		else if ((0x00 == ((opt1->word[i] >> 31) & 0x01)) && (0x01 == ((opt2->word[i] >> 31) & 0x01)))
			dst->word[i] = opt2->word[i];
		else
			dst->f[i] = (opt1->f[i] < opt2->f[i]) ? opt1->f[i] : opt2->f[i]; // f_0 和 f_0_n比较不了
	}
	return true;
}
bool simd_fabs(Reg128 *dst, Reg128 *opt1)
{
	size_t n = sizeof(Reg128) / sizeof(F32);
	for (int i = 0; i < n; i++)
	{
		dst->f[i] = fabs(opt1->f[i]);
	}
	return true;
}
bool simd_fmul(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (int i = 0; i < 4; i++)
	{
		if (is_nan(opt1->word[i]) || is_nan(opt2->word[i]) ||
			(is_inf(opt1->word[i]) && is_zero(opt2->word[i])) ||
			(is_inf(opt2->word[i]) && is_zero(opt1->word[i])))
			dst->word[i] = f_qnan;
		else
			dst->f[i] = opt1->f[i] * opt2->f[i];
	}

	return true;
}
bool simd_fmac_4(Reg32 *dst, Reg128 *opt1, Reg128 *opt2)
{
	Reg128 tmp = {0};

	double c = dst->f;

	for (size_t i = 0; i < 4; i++)
	{
		if (is_nan(opt1->word[i]) || is_nan(opt2->word[i]) ||
			(is_inf(opt1->word[i]) && is_zero(opt2->word[i])) ||
			(is_inf(opt2->word[i]) && is_zero(opt1->word[i])) || is_nan(dst->word))
		{
			dst->word = f_qnan;
			return true;
		}

		double a = opt1->f[i];
		double b = opt2->f[i];

		c += a * b;
	}
	dst->f = c;

	return true;
}

bool simd_i8f32(Reg128 *dst, Reg32 *opt1, Reg32 *opt2)
{
	for (int i = 0; i < 4; i++)
	{
		dst->f[i] = opt1->byte[i] / (float)pow(2.0, 8 - opt2->sword);
	}

	return true;
}

bool simd_i32f32(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	for (int i = 0; i < 4; i++)
	{
		dst->f[i] = opt1->sword[i] / (float)pow(2.0, 31 - opt2->sword);
	}

	return true;
}

bool simd_f32i32(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	for (int i = 0; i < 4; i++)
	{
		S8 s = (0x01 == ((opt1->word[i] >> 31) & 0x01)) ? (-1) : (1);

		double tmp1 = s * fabs(opt1->f[i]) * pow(2, 31 - opt2->word);
		if (INT32_MAX <= tmp1)
			dst->sword[i] = INT32_MAX;
		else if (tmp1 < INT32_MIN)
			dst->sword[i] = INT32_MIN;
		else
			dst->sword[i] = tmp1;
	}

	return true;
}

bool simd_f32i64(Reg128 *dst, Reg64 *opt1, Reg32 *opt2)
{
	for (int i = 0; i < 2; i++)
	{
		S8 s = (0x01 == ((opt1->word[i] >> 31) & 0x01)) ? (-1) : (1);

		double tmp1 = s * fabs(opt1->f[i]) * pow(2, 63 - opt2->word);
		if (INT64_MAX <= tmp1)
			dst->sdword[i] = INT64_MAX;
		else if (tmp1 < INT64_MIN)
			dst->sdword[i] = INT64_MIN;
		else
			dst->sdword[i] = tmp1;
	}

	return true;
}

bool simd_32to16(Reg64 *dst, Reg128 *opt1, Reg32 *opt2)
{
	for (int i = 0; i < 4; i++)
	{
		S32 tmp;
		if (0 == opt2->sword)
		{
			tmp = opt1->sword[i];
			if (tmp > INT16_MAX)
				dst->shword[i] = INT16_MAX;
			else if (tmp < INT16_MIN)
				dst->shword[i] = INT16_MIN;
			else
				dst->shword[i] = tmp;
		}
		if (1 == opt2->sword)
		{
			tmp = round(opt1->sword[i] / pow(2, 16));
			if (tmp > INT16_MAX)
				dst->shword[i] = INT16_MAX;
			else if (tmp < INT16_MIN)
				dst->shword[i] = INT16_MIN;
			else
				dst->shword[i] = tmp;
		}
	}

	return true;
}

bool simd_16to8(Reg64 *dst, Reg128 *opt1, Reg32 *opt2)
{
	for (int i = 0; i < 8; i++)
	{
		if (GET_BIT(opt1->shword[i], 15))
			dst->byte[i] = 0;
		else
		{
			S16 tmp;
			tmp = opt1->shword[i] >> opt2->word;
			if (tmp > UINT8_MAX)
				dst->byte[i] = UINT8_MAX;
			else
				dst->byte[i] = tmp & 0x00FF;
		}
	}

	return true;
}

// bool simd_mac32_4(Reg32 *dst, Reg128 *opt1, Reg128 *opt2)
// {
// 	__int128 tmp = dst->sword * pow(2, 31);
// 	for (int i = 0; i < 4; i++)
// 	{
// 		S64 a = opt1->sword[i];
// 		S64 b = opt2->sword[i];
// 		tmp += a * b;
// 	}
// 	double c = floor(tmp / pow(2, 31));
// 	if (c > INT32_MAX)
// 		dst->sword = INT32_MAX;
// 	else if (c < INT32_MIN)
// 		dst->sword = INT32_MIN;
// 	else
// 		dst->sword = (S32)c;

// 	return true;
// }

bool simd_mac32_4(Reg32 *dst, Reg128 *opt1, Reg128 *opt2)
{
	__int128 tmp = dst->sword;
	S32 product32 = 0;

	for (int i = 0; i < 4; i++)
	{
		S64 a = opt1->sword[i];
		S64 b = opt2->sword[i];

		double c = floor((a * b) / pow(2, 31));
		if (c > INT32_MAX)
			product32 = INT32_MAX;
		else if (c < INT32_MIN)
			product32 = INT32_MIN;
		else
			product32 = (S32)c;
		tmp += product32;
	}
	if (tmp > INT32_MAX)
		dst->sword = INT32_MAX;
	else if (tmp < INT32_MIN)
		dst->sword = INT32_MIN;
	else
		dst->sword = (S32)tmp;

	return true;
}

bool simd_muls32(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	S64 tmp = 0;
	for (int i = 0; i < 4; i++)
	{
		S64 a = opt1->sword[i];
		S64 b = opt2->sword[i];
		tmp = a * b;

		double c = floor(tmp / pow(2, 31));
		if (c > INT32_MAX)
			dst->sword[i] = INT32_MAX;
		else if (c < INT32_MIN)
			dst->sword[i] = INT32_MIN;
		else
			dst->sword[i] = (S32)c;
	}

	return true;
}

bool simd_fshift(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	S32 shift = opt2->sword;
	// double tmp = 0.0;
	for (size_t i = 0; i < 4; i++)
	{
		if (is_nan(opt1->word[i]))
		{
			dst->word[i] = f_qnan;
			continue;
		}
		if (is_inf(opt1->word[i]))
		{
			dst->word[i] = opt1->word[i];
			continue;
		}
		if (is_zero(opt1->word[i]))
		{
			dst->word[i] = opt1->word[i];
			continue;
		}
		float a = opt1->f[i];
		if (shift < 0)
			dst->f[i] = a / pow(2, -shift);
		else
			dst->f[i] = a * pow(2, shift);
	}

	return true;
}
bool simd_frelu(Reg128 *dst, Reg128 *opt1)
{
	for (size_t i = 0; i < 4; i++)
	{
		if (0x01 == ((opt1->sword[i] >> 31) & 0x01))
			dst->f[i] = f_0;
		else
			dst->f[i] = opt1->f[i];
	}

	return true;
}
bool simd_fleaky(Reg128 *dst, Reg128 *opt1)
{
	for (size_t i = 0; i < 4; i++)
	{
		if (0x01 == ((opt1->sword[i] >> 31) & 0x01))
		{
			if (f_snan == opt1->word[i]) // u31与c不一致的地方
				dst->word[i] = f_qnan;
			else
				dst->f[i] = opt1->f[i] / 16;
		}
		else
			dst->f[i] = opt1->f[i];
	}
	return true;
}
/**
 * 特别说明: 该指令多了2个参数reg_s4和reg_s5,实际指令没这个参数,主要是通过程序控制s4和s5的变化
 * reg_s4: L(p-r,d-1)
 * reg_s5: L(p-r,d+1)
 * opt1: P1
 * dst: L(p-r,d)
 * 指令: simd_min_disp s0, r0
 */
bool simd_min_disp(Reg128 *dst, Reg32 *opt1, Reg128 *reg_s4, Reg128 *reg_s5)
{
	Reg128 l_d = *dst;
	Reg128 l_d_pre = *reg_s4;
	Reg128 l_d_nxt = *reg_s5;
	Reg32 p1 = *opt1;

	for (size_t i = 0; i < 8; i++)
	{
		int32_t sum_d_pre = l_d_pre.shword[i] + p1.sword;
		if (sum_d_pre > INT16_MAX)
			sum_d_pre = INT16_MAX;
		if (sum_d_pre < INT16_MIN)
			sum_d_pre = INT16_MIN;
		int32_t sum_d_nxt = l_d_nxt.shword[i] + p1.sword;
		if (sum_d_nxt > INT16_MAX)
			sum_d_nxt = INT16_MAX;
		if (sum_d_nxt < INT16_MIN)
			sum_d_nxt = INT16_MIN;
		dst->shword[i] = std::min(std::min((int32_t)l_d.shword[i], sum_d_pre), sum_d_nxt);
	}

	return true;
}
/**
 * 功能:4路双线性插值
 */
bool simd_interp(Reg32 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (size_t i = 0; i < 4; i++)
	{
		dst->word += opt1->word[i] * opt2->word[i];
	}
	dst->word = (dst->word + 128) >> 8; // 加128用于四舍五入
	return true;
}

bool simd_linear(Reg64 *dst, Reg128 *opt1, Reg32 *opt2)
{
	for (size_t i = 0; i < 4; i++)
	{
		int tmp = 0;
		if (opt2->word < 15)
			tmp = opt1->sword[i] >> opt2->word; // 右移指定位
		else if (15 == opt2->word)
			tmp = opt1->sword[i] >> 16; // 操作数等于15,U31实际移动16位
		else
		{
			printf("opt2 value is invalid! %d\n", opt2->word);
			return false;
		}
		if (tmp > (int)0x00007fff)
		{
			dst->shword[i] = SHRT_MAX; // 右移结果超过16位正数,取极值
		}
		else if (tmp < (int)0xffff8000)
		{
			dst->shword[i] = SHRT_MIN; // 右移结果超过16位负数,取极值
		}
		else
		{
			dst->shword[i] = tmp;
		}
	}
	return true;
}
bool simd_relu(Reg64 *dst, Reg128 *opt1, Reg32 *opt2)
{
	for (size_t i = 0; i < 4; i++)
	{
		int tmp = 0;
		if (opt1->sword[i] <= 0)
		{
			dst->shword[i] = 0;
		}
		else
		{
			if (opt2->word < 15)
				tmp = opt1->sword[i] >> opt2->word; // 右移指定位
			else if (15 == opt2->word)
				tmp = opt1->sword[i] >> 16; // 操作数等于15,U31实际移动16位
			else
			{
				printf("opt2 value is invalid! %d\n", opt2->word);
				return false;
			}
			if (tmp > (int)0x00007FFF)
			{
				dst->shword[i] = SHRT_MAX; // 右移结果超过16位正数,取极值
			}
			else
			{
				dst->shword[i] = tmp;
			}
		}
	}
	return true;
}
bool simd_leaky(Reg64 *dst, Reg128 *opt1, Reg32 *opt2)
{
	for (size_t i = 0; i < 4; i++)
	{
		int tmp = 0;
		if (opt1->sword[i] < 0)
		{
			tmp = opt1->sword[i] >> 4; // 先右移4位
		}
		else
		{
			tmp = opt1->sword[i]; // 不右移
		}
		if (opt2->word < 15)
			tmp = tmp >> opt2->word; // 右移指定位
		else if (15 == opt2->word)
			tmp = tmp >> 16; // 操作数等于15,U31实际移动16位
		else
		{
			printf("opt2 value is invalid! %d\n", opt2->word);
			return false;
		}
		if (tmp > (int)0x00007FFF)
		{
			dst->shword[i] = SHRT_MAX; // 右移结果超过16位正数,取极值
		}
		else if (tmp < (int)0xFFFF8000)
		{
			dst->shword[i] = SHRT_MIN; // 右移结果超过16位负数,取极值
		}
		else
		{
			dst->shword[i] = tmp;
		}
	}
	return true;
}

bool simd_sample(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	int startIdx = 0;
	memset((void *)dst, 0, sizeof(dst));
	if (0 == opt2->sword)
	{
		startIdx = 0;
	}
	else
	{
		startIdx = 8;
	}

	for (int i = 0; i < 8; i++)
	{
		dst->byte[startIdx + i] = opt1->byte[i * 2];
	}

	return true;
}
bool simd_sl(Reg128 *dst, Reg128 *opt1, Reg32 *opt2)
{
	for (int i = 0; i < 16; i++)
	{
		if (opt2->sword >= 0)
		{
			if (i >= opt2->sword)
			{
				dst->byte[i] = opt1->byte[i - opt2->sword];
			}
			else
			{
				dst->byte[i] = 0;
			}
		}
		else
		{
			if (i < 16 + opt2->sword)
			{
				dst->byte[i] = opt1->byte[i - opt2->sword];
			}
			else
			{
				dst->byte[i] = 0;
			}
		}
	}
	return true;
}
bool simd_xor(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (int i = 0; i < 4; i++)
	{
		dst->word[i] = opt1->word[i] ^ opt2->word[i];
	}
	return true;
}
bool simd_and(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (int i = 0; i < 4; i++)
	{
		dst->word[i] = opt1->word[i] & opt2->word[i];
	}
	return true;
}
bool simd_or(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (int i = 0; i < 4; i++)
	{
		dst->word[i] = opt1->word[i] | opt2->word[i];
	}
	return true;
}
bool simd_2bit_to_1bit(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (int i = 0; i < 32; i++)
	{
		dst->word[0] |= ((opt1->dword[0] >> (2 * i + 1)) & 1) ? (0x00000001 << i) : 0x00000000;
		dst->word[1] |= ((opt1->dword[1] >> (2 * i + 1)) & 1) ? (0x00000001 << i) : 0x00000000;
		dst->word[2] |= ((opt2->dword[0] >> (2 * i + 1)) & 1) ? (0x00000001 << i) : 0x00000000;
		dst->word[3] |= ((opt2->dword[1] >> (2 * i + 1)) & 1) ? (0x00000001 << i) : 0x00000000;
	}
	return true;
}
bool simd_1bit_to_2bit(Reg128 *dst, Reg64 *opt1)
{
	for (int i = 0; i < 32; i++)
	{
		dst->word[0] |= ((opt1->hword[0] >> i) & 1) ? (0x00000001 << (i * 2)) : 0x00000000;
		dst->word[1] |= ((opt1->hword[1] >> i) & 1) ? (0x00000001 << (i * 2)) : 0x00000000;
		dst->word[2] |= ((opt1->hword[2] >> i) & 1) ? (0x00000001 << (i * 2)) : 0x00000000;
		dst->word[3] |= ((opt1->hword[3] >> i) & 1) ? (0x00000001 << (i * 2)) : 0x00000000;
	}
	return true;
}
bool simd_muls16(Reg128 *dst, Reg64 *opt1, Reg64 *opt2)
{
	for (int i = 0; i < 4; i++)
	{
		dst->sword[i] = opt1->shword[i] * opt2->shword[i];
	}
	return true;
}
bool simd_mac16_4(Reg32 *dst, Reg64 *opt1, Reg64 *opt2)
{
	int64_t tmp = dst->sword;
	for (int i = 0; i < 4; i++)
	{
		tmp += opt1->shword[i] * opt2->shword[i];
	}
	if (tmp > INT32_MAX)
		dst->sword = INT32_MAX;
	else if (tmp < INT32_MIN)
		dst->sword = INT32_MIN;
	else
		dst->sword = tmp;

	return true;
}
bool simd_add32(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	S64 tmp_opt1, tmp_opt2, sum;

	for (size_t i = 0; i < 4; i++)
	{
		tmp_opt1 = opt1->sword[i];
		tmp_opt2 = opt2->sword[i];
		sum = tmp_opt1 + tmp_opt2;
		if (sum < INT_MIN)
			sum = INT_MIN;
		if (sum > INT_MAX)
			sum = INT_MAX;
		dst->sword[i] = sum;
	}

	return true;
}
bool simd_sub32(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	S64 tmp_opt1, tmp_opt2, sum;

	for (size_t i = 0; i < 4; i++)
	{
		tmp_opt1 = opt1->sword[i];
		tmp_opt2 = opt2->sword[i];
		sum = tmp_opt1 - tmp_opt2;
		if (sum < INT_MIN)
			sum = INT_MIN;
		if (sum > INT_MAX)
			sum = INT_MAX;
		dst->sword[i] = sum;
	}
	return true;
}
bool simd_add16(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	S64 tmp_opt1, tmp_opt2, sum;

	for (size_t i = 0; i < 8; i++)
	{
		tmp_opt1 = opt1->shword[i];
		tmp_opt2 = opt2->shword[i];
		sum = tmp_opt1 + tmp_opt2;
		if (sum < SHRT_MIN)
			sum = SHRT_MIN;
		if (sum > SHRT_MAX)
			sum = SHRT_MAX;
		dst->shword[i] = sum;
	}
	return true;
}
bool simd_sub8(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (size_t i = 0; i < 16; i++)
	{
		if (opt1->byte[i] < opt2->byte[i])
			dst->byte[i] = 0;
		else
			dst->byte[i] = opt1->byte[i] - opt2->byte[i];
	}
	return true;
}
bool simd_max16(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (size_t i = 0; i < 8; i++)
	{
		dst->shword[i] = (opt1->shword[i] < opt2->shword[i]) ? opt2->shword[i] : opt1->shword[i];
	}

	return true;
}
bool simd_min16(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (size_t i = 0; i < 8; i++)
	{
		dst->shword[i] = (opt1->shword[i] < opt2->shword[i]) ? opt1->shword[i] : opt2->shword[i];
	}
	return true;
}
bool simd_max16_mak(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (size_t i = 0; i < 8; i++)
	{
		dst->shword[i] = (opt1->shword[i] < opt2->shword[i]) ? 0x0000 : 0xFFFF;
	}
	return true;
}
bool simd_max8(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (size_t i = 0; i < 16; i++)
	{
		dst->byte[i] = (opt1->byte[i] < opt2->byte[i]) ? opt2->byte[i] : opt1->byte[i];
	}
	return true;
}
bool simd_min8(Reg128 *dst, Reg128 *opt1, Reg128 *opt2)
{
	for (size_t i = 0; i < 16; i++)
	{
		dst->byte[i] = (opt1->byte[i] < opt2->byte[i]) ? opt1->byte[i] : opt2->byte[i];
	}
	return true;
}
bool simd_shuffle64(Reg64 *dst, Reg64 *opt1)
{
	dst->shword[0] = opt1->shword[0];
	dst->shword[1] = opt1->shword[2];
	dst->shword[2] = opt1->shword[1];
	dst->shword[3] = opt1->shword[3];

	return true;
}
bool simd_shuffle128(Reg128 *dst, Reg128 *opt1)
{
	dst->shword[0] = opt1->shword[0];
	dst->shword[1] = opt1->shword[2];
	dst->shword[2] = opt1->shword[4];
	dst->shword[3] = opt1->shword[6];
	dst->shword[4] = opt1->shword[1];
	dst->shword[5] = opt1->shword[3];
	dst->shword[6] = opt1->shword[5];
	dst->shword[7] = opt1->shword[7];
	return true;
}
bool simd_max16_8(Reg32 *dst, Reg128 *opt1)
{
	S16 tmp = opt1->shword[0];
	for (size_t i = 0; i < 8; i++)
	{
		if (tmp < opt1->shword[i])
			tmp = opt1->shword[i];
	}
	dst->shword[0] = tmp;
	dst->shword[1] = (tmp < 0) ? 0xFFFF : 0x0000;
	return true;
}
bool simd_min16_8(Reg32 *dst, Reg128 *opt1)
{
	S16 tmp = opt1->shword[0];
	for (size_t i = 0; i < 8; i++)
	{
		if (tmp > opt1->shword[i])
			tmp = opt1->shword[i];
	}
	dst->shword[0] = tmp;
	dst->shword[1] = (tmp < 0) ? 0xFFFF : 0x0000;
	return true;
}
